#ifndef COBS_H_INCLUDED
#define COBS_H_INCLUDED
/******************************************************************************
* (c)2025 Michael T. Richter
*
* This software is distributed under the terms of WTFPLv2.  The full terms and
* text of the license can be found at http://www.wtfpl.net/txt/copying
******************************************************************************/
/*******************************************************************************
* COBS - Yet another Consistent Overhead Byte Stuffing implementation.
*
* Ref: https://en.wikipedia.org/wiki/Consistent_Overhead_Byte_Stuffing
*
* In short, COBS encodes any buffer of bytes in such a way that there are no
* zero bytes, allowing any encoded buffer to be handled with NUL termination
* without difficulties.  It's a very useful format for things like transmitting
* data over serial ports with easy detection of the end of a packet (NUL
* separation) and other such use cases with minimal added overhead.
*******************************************************************************/

#include <stdbool.h>
#include <stddef.h>
#include <stdint.h>

#ifndef UNUSED
#define UNUSED(X) ((void)X)
#endif

/*
 * Errors are signalled by calling `cobs_error_handler()`.  This is a function
 * pointer allowing a user to override error handling by merely assigning
 * another function to the pointer.
 */
typedef void (*cobs_error_handler_t)(const char *file, const int line, const char *error_message);
void default_cobs_error_handler(const char *file, const int line, const char *error_message);
extern cobs_error_handler_t cobs_error_handler;

/*
 * Decode an arbitrary-sized buffer.
 *
 * idata -> the COBS-encoded input buffer
 * size  -> the size of the input data in bytes
 * odata -> the location of the decoded data buffer
 *
 * Returns the size of the decoded data buffer in bytes, 0 on failure.
 */
size_t cobs_decode(const uint8_t *idata, size_t size, void *odata);

/*
 * Encode an arbitrary-sized buffer.
 *
 * idata -> the arbitrary input data buffer
 * size  -> the size of the input data in bytes
 * odata -> the location of the encoded data buffer
 *
 * Returns the size of the encoded data buffer in bytes, 0 on failure.
 */
size_t cobs_encode(const void *idata, size_t size, uint8_t *odata);

#ifdef COBS_IMPLEMENTATION
/*******************************************************************************
* COBS IMPLEMENTATION SECTION
* ==============================
*
* In a single source file in your project, the symbol COBS_IMPLEMENTATION
* must be defined before including the header:
*
*   ```c
*   #define COBS_IMPLEMENTATION
*   #include "cobs.h"
*   ```
*
* All other source files using this library must *not* define this symbol.
*******************************************************************************/
#ifdef COBS_DEBUG
#define NL()            printf("\n");
#define OUTPUT(M)       printf("DEBUG %s %d: %s\n", __FILE__, __LINE__, M)
#define OUTPUT_(M, ...) printf("DEBUG %s %d: " M "\n", __FILE__, __LINE__, __VA_ARGS__)
#else
#define NL()
#define OUTPUT(...)
#define OUTPUT_(...)
#endif //COBS_DEBUG

cobs_error_handler_t cobs_error_handler = default_cobs_error_handler;

size_t cobs_decode(const uint8_t *idata, size_t size, void *odata)
{
    NL() ; OUTPUT_("cobs_decode(idata=%p, size=%lu, odata=%p)", idata, size, odata);
    if (!(idata && odata))
    {
        cobs_error_handler(__FILE__, __LINE__, "both input and output data buffers must be provided");
        return 0;
    }

    const uint8_t *read_index = idata;
    uint8_t *write_index = (uint8_t *)odata;
    const uint8_t *end = idata + size;

    while (read_index < end)
    {
        /* get the number of bytes until the next 0 */
        uint8_t code = *read_index++;
        /* transfer each plain byte */
        for (uint8_t i = 1; i < code; i++)
        {
            if (read_index >= end) {
                cobs_error_handler(__FILE__, __LINE__, "malformed encoded data");
                return 0;
            }
            *write_index++ = *read_index++;
        }
        /* code FF means just that 254 bytes got processed       */
        /* for anything else, we write a 0 in the assigned space */
        if (code < 0xFF && read_index < end)
        {
            *write_index++ = 0;
        }
    }

    /* report the bytes decoded, without the trailing 0 */
    return (size_t)(write_index - (uint8_t *)odata) - 1;
}

size_t cobs_encode(const void *idata, size_t size, uint8_t *odata)
{
    NL() ; OUTPUT_("cobs_encode(idata=%p, size=%lu, odata=%p)", idata, size, odata);
    if (!(idata && odata))
    {
        cobs_error_handler(__FILE__, __LINE__, "both input and output data buffers must be provided");
        return 0;
    }

    uint8_t *encp = odata;      /* encoding pointer       */
    uint8_t *codep = encp++;    /* zero byte data pointer */
    uint8_t coffs = 1;          /* code offset            */

    for (const uint8_t *ibuf = (const uint8_t *)idata; size--; ++ibuf)
    {
        /* byte is not zero */
        if (*ibuf)
        {
            /* copy it into encode buffer and update the code offset */
            *encp++ = *ibuf;
            ++coffs;
        }

        /* byte is zero, or we've filled a block */
        /* either way we restart all the counters */
        if (!*ibuf || coffs == 0xff)
        {
            *codep = coffs;         /* show where the zero byte is */
            coffs = 1;              /* reset the zero byte offset  */
            codep = encp;           /* point to where encp is      */

            /* if we had a zero or if there's more data, update */
            if (!*ibuf || size)
            {
                ++encp;
            }
        }
    }

    /* write the final code value */
    *codep = coffs;

    /* report the bytes written, including the trailing 0 */
	return (size_t)(encp - odata) + 1;
}

/*******************************************************************************
* Static helper function definitions
*******************************************************************************/

void default_cobs_error_handler(const char *file, const int line, const char *error_message)
{
    printf("ERROR: %s(%d): %s\n", file, line, error_message);
}

#endif //COBS_IMPLEMENTATION

#endif /*COBS_H_INCLUDED*/
